#include "hitechnic-sensormux.h"
#include "hitechnic-colour-v2.h"
#include "hitechnic-irseeker-v2.h"
#include "hitechnic-gyro.h"
#include "lego-touch.h"
#include "lego-ultrasound.h"
#include "hitechnic-eopd.h"
#include "AccelHeader.h"

//Declare variables and constants
#define CONVERSION_1_INCH 91  //constants for converting distance to encoders
#define SERVO_LEFT_UP  0//Constant positions for servos
#define SERVO_LEFT_DOWN 91
#define KICK_OUT 60
#define KICK_IN 180
#define REDIRECT_UP 128
#define REDIRECT_MID 0
#define REDIRECT_DOWN 65
#define CONVERSION_1_DEGREE 13.4
#define DISTANCE_1_INCH 2.73
#define CENTER_GOAL_DOWN 36
#define CENTER_GOAL_UP 255
#define LIFT_TIME 12000

const tMUXSensor HTCOLOR = msensor_S2_1;
const tMUXSensor HTIRS2 = msensor_S2_2;
const tMUXSensor HTEOPD = msensor_S2_3;


int color = 0;
int dist = 0;
float heading = 0;
float rotateSpeed = 0;
float accelX = 0;
float accelY = 0;
int distEOPD = 0;
//int headingDegrees = 0;
int rawIRdir = 0;

//Declare functions
void translateDistanceBased(int motorPower, int distanceValue, int moveAngle);

void rotateClockwiseAngleBased(int motorPower, int angle);

void rotateCounterClockwiseAngleBased(int motorPower, int angle);

void rotateCWGyro(int motorPower, int angle);

void rotateCCWGyro(int motorPower, int angle);

void halt();

void toggleHook();

void getIRDirection();

task updateSensors();

task updateGyro();

void hookPosition();

int getEncoderValueForAngle(int angleDegrees);

int getEncoderValueForDistance(int distanceInches);

void toggleKick();

typedef enumWord//Enum for storing the position of the hook servos
{
	Down, Up, Mid
} HookPosition;

HookPosition servoLeft = Up;

typedef enumWord//Enum for storing the center goal position
{
	One, Two, Three, Error
} CenterGoalPosition;

CenterGoalPosition dir = Error;

void translateDistanceBased(int motorPower, int distanceValue, int moveAngle)//Moves the robot forward at a set power for a distance.
{
	nMotorEncoder[Right] = 0;
	nMotorEncoder[Front] = 0;
	nMotorEncoder[Rear] = 0;
	nMotorEncoder[Left] = 0;
	float currPower = 0;
	while (abs(nMotorEncoder[Right]) < getEncoderValueForDistance(distanceValue) && abs(nMotorEncoder[Rear]) < getEncoderValueForDistance(distanceValue))
	{
		if (currPower < motorPower)
		{
			currPower += (motorPower / 10);
			wait10Msec(7);
		}
		motor[Left] = (cosDegrees(moveAngle) * motorPower);//Sets each motor based on the angle parameter
		motor[Right] = (-cosDegrees(moveAngle) * motorPower);
		motor[Front] = (sinDegrees(moveAngle) * motorPower);
		motor[Rear] = (-sinDegrees(moveAngle) * motorPower);
	}
	halt();
}

void rotateClockwiseAngleBased(int motorPower, int angle)//Rotates the robot clockwise using encoders.
{
	nMotorEncoder[Right] = 0;
	float currAngle = nMotorEncoder[Right];
	float currPower;
	float angleConversion = (float)(motorPower - 5)/angle;
	while (abs(nMotorEncoder[Right]) < getEncoderValueForAngle(angle))
	{
		currPower = ((angle - currAngle) * angleConversion) + 5;//Decreases the motor power as the robot reaches its desired angle
		currAngle = abs(nMotorEncoder[Right]) / CONVERSION_1_DEGREE;//Converts the encoder value to an angle
		motor[Left] = currPower;
		motor[Right] = currPower;
		motor[Front] = currPower;
		motor[Rear] = currPower;
	}
	halt();
}

void rotateCounterClockwiseAngleBased(int motorPower, int angle)//Rotates the robot counter-clockwise using encoders.
{
	nMotorEncoder[Right] = 0;
	float currAngle = nMotorEncoder[Right];
	float currPower;
	float angleConversion = (float)(motorPower - 5)/angle;
	while (abs(nMotorEncoder[Right]) < getEncoderValueForAngle(angle))
	{
		currPower = ((angle - currAngle) * angleConversion) + 5;//Decreases the motor power as the robot reaches its target angle
		currAngle = abs(nMotorEncoder[Right]) / CONVERSION_1_DEGREE;//Convert the encoder value to an angle
		motor[Left] = -currPower;
		motor[Right] = -currPower;
		motor[Front] = -currPower;
		motor[Rear] = -currPower;
	}
	halt();
}

void rotateCWGyro(int motorPower, int angle)//Rotates CW to a certain gyro position
{
	float angleDifference = angle - heading;
	float currPower;
	float angleConversion = (float)(motorPower - 30)/angleDifference;
	while (heading < angle)//Compensates for the robot stopping time.
	{
		currPower = ((angle - heading) * angleConversion) + 30;
		motor[Left] = currPower;
		motor[Right] = currPower;
		motor[Front] = currPower;
		motor[Rear] = currPower;
	}
	halt();

}

void rotateCCWGyro(int motorPower, int angle)//Rotates CCW to a certain gyro position.
{
	float angleDifference = heading - angle;
	float currPower;
	float angleConversion = (float)(motorPower - 30)/angleDifference;
	while (heading > angle)//Compensates for the robot stopping time.
	{
		currPower = (abs((angle - heading)) * angleConversion) + 30;
		motor[Left] = -currPower;
		motor[Right] = -currPower;
		motor[Front] = -currPower;
		motor[Rear] = -currPower;
	}
	halt();
}

void halt() // stops all drive motors
{
	motor[Left] = 0;
	motor[Right] = 0;
	motor[Front] = 0;
	motor[Rear] = 0;
}

void toggleHook()//Toggles the position of the hook servo
{
	hookPosition();
	if(servoLeft == Down )//Resets hook
	{

		servo[Hook] = SERVO_LEFT_UP;
	}
	else//Puts hook down
	{
		servo[Hook] = SERVO_LEFT_DOWN;
	}
}

void getIRDirection()//Sets the enum for storing the center goal position
{

	if (rawIRdir == -1)//IR sensor error
	{
		dir = Error;
	}
	else if (rawIRdir == 0)//Center goal position one
	{
		dir = One;
	}
	else if (rawIRdir <= 4)//Center goal position two
	{
		dir = Two;
	}
	else
	{
		dir = Three;//Center goal position three
	}
}

task updateSensors()//Updates the value of the other sensors
{
	while (true)
	{
		rawIRdir = HTIRS2readACDir(HTIRS2);
		color = HTCS2readColor(HTCOLOR);
		//dist = USreadDist(Sonic);
		distEOPD = HTEOPDreadProcessed(HTEOPD);

		updateVals();
		accelX = getX();
		accelY = getY();

	}
	EndTimeSlice();
}

task updateGyro()//Constantly updates the gyro heading
{
	float driftCompensate = 0;
	for (int i = 0; i < 10; i++)//Averages gyro drift
	{
		driftCompensate += HTGYROreadRot(HTGYRO);
		wait10Msec(5);
	}
	driftCompensate /= 10;
	while (true)
	{
		rotateSpeed = HTGYROreadRot(HTGYRO) - driftCompensate;
		if (abs(rotateSpeed) < 1)//Compensate for drift
		{
			rotateSpeed = 0;
		}
		heading += (float)(rotateSpeed * 0.015);
		wait1Msec(15);
	}
}

void hookPosition()//Updates the hook position enum based on servo values
{

	if (ServoValue[Hook] == SERVO_LEFT_UP)
	{
		servoLeft = Up;
	}
	else if (ServoValue[Hook] == SERVO_LEFT_DOWN)
	{
		servoLeft = Down;
	}
}

int getEncoderValueForAngle(int angleDegrees) // converts an angle to an encoder value
{
	return angleDegrees * CONVERSION_1_DEGREE;
}

int getEncoderValueForDistance(int distanceInches)//Converts a distance in inches to an encoder value.
{
	return distanceInches * CONVERSION_1_INCH;
}

void toggleKick()
{
	if (ServoValue[KickArm] == KICK_OUT)
	{
		servo[KickArm] = KICK_IN;
	}
	else
	{
		servo[KickArm] = KICK_OUT;
	}
}
